#ifndef COOLPOOL_SIMULATION_WORLDGRID_H_
#define COOLPOOL_SIMULATION_WORLDGRID_H_

#ifndef EIGEN_DONT_ALIGN 
#define EIGEN_DONT_ALIGN 
#endif

#ifndef EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT 
#define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#endif

#include <eigen3/Eigen/Dense>
#include <queue>
#include <set>

#include "EntitiesContainer.h"
#include "WorldCell.h"

namespace coolpool
{
	typedef std::vector<WorldCell> WorldGridRow;
	typedef std::vector<WorldGridRow> WorldGridMatrix;
	typedef std::pair<DynamicEntity *, Entity *> CollisionPair;
	typedef std::queue<CollisionPair> CollisionPairsQueue;
	typedef std::set<CollisionPair> CollisionPairsSet;

	// World Grid which splits the world into cells. This grid horizontal ant it's orientation is
	// unchangeable. The bottom left corner is in the center of coordinate system (0,0), which is also
	// unchangeable. So needs to lie within the positive x and z axis. The objects within this
	// cube are ignored by the collision detection engine. To select the right cell of the object,
	// its bounding cylinder is used.
	//
	// Initialisation:
	// The setup of the grid is on the user, nothing is done automatically
	//
	// The object which are bigger then the sell are stored separately, and we say
	// that they may be in contact with each dynamic object. This is necessary for handling
	// flors, walls etc.
	class WorldGrid
	{
	public:

		// Builds the world grid
		// width - number of columns, the Z axis I think :)
		// length - number of rows
		// cell_size - size of one cell
		// entities_container - list of elements from which the grid is builded.
		WorldGrid(int width, int length, double cell_size, EntitiesContainer * entities_container);
		virtual ~WorldGrid();

		// Builds the world rgrid from the given object.
		// all - if true all the objects are recalculated, otherwise only dynamic objects.
		void buildWorldGrid(bool all);

		// Runs over all the cells and large objects and creates pairs between which it is necessary
		// to check for collision.
		CollisionPairsQueue * getCollisionPairs();

		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	protected:

		// Clears all the cells.
		void clearAll();

		// Clears only the dynamic containers of cells.
		void clearDynamics();

		// Clears only the static containers of cells.
		void clearStatics();

		// Puts the given entity to the right cell.
		void enqueue(Entity * entity);

		// Adds the collision pairs from given cell to the list.
		void addCollisionPairsFromCell(WorldCell * cell, CollisionPairsQueue * collision_pair_list, CollisionPairsSet * already_added);

		// Adds entity to the given cell.
		inline void addToCell(int row, int col, Entity * entity)
		{
			if (WorldCell * cell = getCell(row,col))
				cell->addEntity(entity);
		}

		inline void addToQueue(CollisionPairsSet *already_added, CollisionPair current, CollisionPairsQueue *collision_pair_list)
		{
			if (already_added->find(current) == already_added->end())
			{
				already_added->insert(current);
				collision_pair_list->push(current);
			}
		}

		// Simple distance calculation.
		inline double distanceSqr(Eigen::Vector2d & point, double x, double y)
		{
			return (math_tools::sqr(point[0] - x) + math_tools::sqr(point[1] - y));
		}

		// Returns the world cell, invalid ones are ignored and NULL is returned.
		inline WorldCell * getCell(int row, int col)
		{
			if (row < length_ && col < width_ && row >= 0 && col >= 0)
			{
				return (&(world_grid_[row])[col]);
			}
			else
			{
				return NULL;
			}
		}

	private:

		// Access to entities.
		EntitiesContainer * entities_container_;
 
		// Container of big entities.
		EntitiesPointers big_statics_;

		// Center of coors system.
		Eigen::Vector2d bottom_left_;

		// Cell size.
		double cell_size_;

		// The representation of the grid.
		WorldGridMatrix world_grid_;

		// Length/width, Z/X axis
		int length_;
		int width_;

		// Initialisation of the grid.
		void init();
	};

}

#endif
